/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Module for handling multiple connections and their cooperation.
 * \author Antti Maula <antti.maula@tkk.fi>
 *
 */
#include "owndebug.h"
#include "ownutils.h"
#include "connectionList.h"
#include "protocol/hubctrl.h"
#include <assert.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>


//*****************************************************************************
//Define the IDMAP hashtable size to be one page. (One entry is 8
//bytes, so shift 3 bits to get this value)
#define CONNECTIONLIST_IDMAP_HASHTABLE_SIZE (sysconf(_SC_PAGESIZE)>>3)
//*****************************************************************************
typedef enum { connectionListLookup_Singletarget = 0,
               connectionListLookup_BroadcastClients,
               connectionListLookup_BroadcastHubs,
               connectionListLookup_NoTarget } EConnectionListLookupStatus;

//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
static uint32_t getNewLocalClientID(TConnectionList *cl);

//*****************************************************************************
inline static int connectionListAddEntry(TConnectionList *cl,
                                         const uint32_t aID,
                                         TConnectionListEntry *cle);
inline static int connectionListDeleteEntry(TConnectionList *cl,
                                            const uint32_t aID);
inline static TConnectionListEntry *connectionListGetEntry(TConnectionList *cl,
                                                           const uint32_t aID,
                                                           const int direct_only);
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

TConnectionList *connectionListCreate()
{
  // Create a new connectionList
  TConnectionList *newList = (TConnectionList*)malloc(sizeof(TConnectionList));
  assert(newList != NULL);

  // Call the init function for this entry
  if (connectionListInit(newList) != 0) {
    free(newList);
    newList = NULL;
  }

  // Return the pointer, whatever it currently is (may be NULL if Init failed)
  return newList;
}
//*****************************************************************************

int connectionListInit( TConnectionList *cl )
{
  // Trivial check 
  assert(cl != NULL);

  dPrint(4, "Initializing connectionList");

  // Init the connectionList protector mutex
  pthread_mutex_init(&cl->mutex, NULL); 

  // Addressing
  cl->localClientSequence = 0x0F; // First local connection is 101, next 1022.. etc.
  cl->localHubID = CONNECTIONLIST_DEFAULT_HUBID; // Own HUB id

  // Initialize list for Hubs & Clients
  idMapInit(&cl->connectionMap, CONNECTIONLIST_IDMAP_HASHTABLE_SIZE);

#if defined(USE_SELECT_IO)
  // Init sets to zero.
  FD_ZERO(&cl->read_set);
  FD_ZERO(&cl->write_set);
  FD_ZERO(&cl->exception_set);

#elif defined(USE_EPOLL_IO) 
  cl->epoll_fd = epoll_create(20);
  if (cl->epoll_fd < 0) {
    dPrintLE(1,"Failed to initialize EPOLL mechanism!");
  }

#endif  

  return CONNECTIONLIST_OK;
}
//*****************************************************************************

void connectionListDeinit( TConnectionList *cl )
{
  assert(cl != NULL);

  pthread_mutex_lock(&cl->mutex);
  pthread_mutex_unlock(&cl->mutex);

  // Cleanup
  pthread_mutex_destroy(&cl->mutex);

  return;
}
//*****************************************************************************

void connectionListDestroy( TConnectionList *cl )
{
  connectionListDeinit(cl);
  free(cl);

  return;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//** Conditional code block : Using select() based IO.
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
#if defined(USE_SELECT_IO)

static int connectionListCreateSelectSets(TConnectionList *cl,
                                          fd_set *readset,
                                          fd_set *writeset,
                                          fd_set *exceptionset)
{
  //# warning This is a unclean and unoptimized implementation!
  int highest_fd = -1;

  TIDMapEntryIterator ei;
  TConnectionListEntry *entry;

  if (cl->highest_set_fd > 0) {
    // Connection table set not changed, so just copy the existing
    // sets and assign the highest set value.
    //    dPrint(ODTEST,"Using pregenerated select() tables");
    *readset = cl->read_set;
    *writeset = cl->write_set;
    *exceptionset = cl->exception_set;
    highest_fd = cl->highest_set_fd;

  } else {
    //    dPrint(ODTEST,"Regenerating select() tables (cl->highest_set_fd : %d)", cl->highest_set_fd);

    // Clear the shared sets.
    FD_ZERO(&cl->read_set);
    FD_ZERO(&cl->write_set);
    FD_ZERO(&cl->exception_set);
    
    // Get iterator to beginning of connection list.
    connectionListGetConnectionIterator(cl, &ei);
    
    // Get connections all in order and check the connection for
    // events.
    while( (entry = connectionListGetNextConnection(cl, &ei)) ) {
      
      // Store some values in local variables for fast access.
      int opr = 0;
      const unsigned int entry_flags = entry->flags;
      const int fd = connectionGetHandle(entry->connection);
      
      // Check for fd (whether its greater than existing or not)
      if (fd > highest_fd) {
        highest_fd = fd;
      }
      
      // Check if READ flag is requested, if so; add to read_set
      if (entry_flags & connectionEntryFlag_SelectSetRead) {
        FD_SET(fd, &cl->read_set);
      }
      
      // Check if WRITE flag is requested, if so; add to write_set
      if (entry_flags & connectionEntryFlag_SelectSetWrite) {
        FD_SET(fd, &cl->write_set);
      }
      
      // Check if EXCEPTION flag is requested, if so; add to
      // exception_set
      if (entry_flags & connectionEntryFlag_SelectSetException) {
        FD_SET(fd, &cl->exception_set);
      }
    }
    
    // Copy resulting sets, and store highest fd.
    *readset = cl->read_set;
    *writeset = cl->write_set;
    *exceptionset = cl->exception_set;
    cl->highest_set_fd = highest_fd;
    
  }
  
  //  dPrint(ODTEST,"Sets constructed, highest_fd = %d", highest_fd);

  return highest_fd + 1;
}
//*****************************************************************************
                                   
int connectionListDoSelect( TConnectionList *cl,
                            fd_set *readset,
                            fd_set *writeset,
                            fd_set *exceptionset,
                            unsigned int timeout_ms)
{
  int result = -1;
  int nfds;
  assert(cl != NULL);
  assert(readset != NULL);
  assert(writeset != NULL);
  assert(exceptionset != NULL);

  // Construct sets.
  nfds = connectionListCreateSelectSets(cl, readset, writeset, exceptionset);

  if (nfds > 0) {
    // Do select()
    struct timeval tv;
    tv.tv_usec = 1000 * (timeout_ms % 1000);
    tv.tv_sec = timeout_ms / 1000;
    
    result = select(nfds, readset, writeset, exceptionset, &tv);

    //    dPrint(ODTEST,"select() returned %d", result);
  }
  
  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//** Conditional code block : Using select() based IO.
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

#elif defined(USE_EPOLL_IO)

int connectionListDoEpollWait( TConnectionList *cl,
                               struct epoll_event *events,
                               unsigned int aMaxEvents,
                               unsigned int timeout_ms)
{
  int result = -1;
  return result;
}
#endif
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

TConnectionListEntry *connectionListAddConnection( TConnectionList *cl,
                                                   EConnectionType type,
                                                   EConnectionState state,
                                                   EConnectionProtocol protocol,
                                                   unsigned int flags,
                                                   TConnection *nC)
{
  
  // Private variables
  int result = -1;
  uint32_t newID;
  TConnectionListEntry *newEntry = NULL;
  
  // Do some trivial checks
  assert(cl != NULL);
  assert(nC != NULL);
  
  // Get new ClientID.
  newID = getNewLocalClientID(cl);
  
  // Test the new ID for validity
  if (newID == 0) {
    dPrint(1,"Unable to construct new clientID entry! Maybe client list full?");
    return NULL; // Nothing locked, so exit here is safe.

  } else {
    
    newEntry = connectionListEntryCreate(newID, type, state, 
                                         protocol, flags, nC);
    assert(newEntry != NULL);
    
    // Now call the next func in stack.
    pthread_mutex_lock(&cl->mutex);
    
    // Add entry to connectionList
    result = connectionListAddEntry(cl, newID, newEntry);
    
    //# warning Generate something? Or later?
    // FIXME: Generate something?
    
    // Release mutex
    pthread_mutex_unlock(&cl->mutex);

  }
  
  return newEntry;
}
//*****************************************************************************

int connectionListRemoveConnection( TConnectionList *cl, 
                                    uint32_t ID )
{
  
  dPrint(4,"Attempting to search and destroy HUB with ID 0x%08x",
         ID);
  
  // Lock;
  pthread_mutex_lock(&cl->mutex);
  
  // because the 'ce' is destroyed in calling ;) FIXME: Constly correct?
  connectionListDeleteEntry(cl, ID);
  
#if defined(USE_SELECT_IO)
  // Mark highest fd zero to force regeneration.
  cl->highest_set_fd = 0;
  
#elif defined(USE_EPOLL_IO)
#endif
  
  // Unlock
  pthread_mutex_unlock(&cl->mutex);

  // DONE!
  return CONNECTIONLIST_OK;

}
//*****************************************************************************

int connectionListGetConnectionIterator(TConnectionList *cl,
                                        TIDMapEntryIterator *aIter)
{
  int result = 0;
 
  // Just pass in the call to idMap.
  idMapGetNextEntryIterator(&cl->connectionMap, aIter);
  
  return result;
}
//*****************************************************************************

TConnectionListEntry *connectionListGetNextConnection(TConnectionList *cl, 
                                                      TIDMapEntryIterator *aIter)
{
  // Return result.
  TConnectionListEntry *cle = NULL;

  // Do look on ime.
  const TIDMapEntry *ime = idMapGetNextEntry(&cl->connectionMap, aIter);
  
  // if ime is valid, return the stored entry.
  if (ime) {
    cle = (TConnectionListEntry *)ime->entry;
  }
  
  return cle;
}
//*****************************************************************************

void connectionListUpdateEntryFlags(TConnectionList *cl,
                                    TConnectionListEntry *cle,
                                    const unsigned int aNewFlags)
{
  assert(cl != NULL);
  assert(cle != NULL);

  // Do update action.
#if defined(USE_SELECT_IO)
  // Mark highest fd zero to force regeneration.
  cl->highest_set_fd = 0;
  
#elif defined(USE_EPOLL_IO)

#endif


  // set flags.
  cle->flags = aNewFlags;
}
//*****************************************************************************

int connectionListReassignConnectionID(TConnectionList *cl, 
                                       const uint32_t from, 
                                       const uint32_t to) 
{
  int result = CONNECTIONLIST_ERROR;

  // Check that from and to HUB IDs match, and are local.
  if (ID2HUBID(from) == ID2HUBID(to) &&
      ID2HUBID(from) == ID2HUBID(cl->localHubID)) {
        
    // Find entry from table.
    TConnectionListEntry *existing_entry = connectionListGetEntry(cl,
                                                                  from,
                                                                  1);
    // Did we find the entry?
    if (existing_entry) {
      int r;
      dPrint(ODTEST,"Found existing entry: %p, has Id %08x",
             existing_entry, existing_entry->connectionID);

      // Check once more whether the 'to' entry already exists?
      const TConnectionListEntry *existing_to_entry = connectionListGetEntry(cl,
                                                                             to,
                                                                             1);
      
      // FIXME: Check the 'to' entry for real validity, avoids
      // reassignment problems!
      if (existing_to_entry) {
        // 'To' entry already exists, hence we cannot change the ID.
        dPrint(ODTEST,"'To' entry '0x%08x' already exists. Refusing to override.", to);
        
      } else {

        // Entry was really found. So proceed with deletion. (This really
        // does the entry search twice, but as Reassign is not done very
        // frequently, it doesn't matter)
        r = connectionListDeleteEntry(cl, from);
        assert(r == 0); // Because entry was found previously, this must go OK.
        dPrint(ODTEST,"Deleting old entry was succesfull.");

        // Change ID
        existing_entry->connectionID = to;
        existing_entry->flags |= connectionEntryFlag_SkipOnce;

        // Re-add entry
        r = connectionListAddEntry(cl, to, existing_entry);
        assert(r == 0);
        dPrint(ODTEST,"Adding entry again was succesfull");
        
        const TConnectionListEntry *testy = connectionListGetEntry(cl,
                                                                   existing_entry->connectionID,
                                                                   1);
        assert(testy != NULL);
        assert(testy->connectionID == to);
        assert(testy == existing_entry);


        // Mark result ok.
        result = CONNECTIONLIST_OK;

      }

    }
  
  } else {
    dPrint(ODTEST,"Reassign failed: 'from(%08x)' and/or to(%08x) are not from local(%08x) HUB",
           from, to, cl->localHubID);
  
  }
  
  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

int connectionListSetLinkDelayByEntry(TConnectionList *cl, 
                                      TConnectionListEntry *entry,
                                      const int delay_ms)
{
  //#warning connectionListSetLinkDelayByEntry - Unimplemented
  dPrint(ODTEST,"UNIMPLEMENTED!");
  return -1;
}
//*****************************************************************************

int connectionListSetLinkDelayByID(TConnectionList *cl, 
                                   const int link_id, 
                                   const int delay_ms)
{
  //#warning connectionListSetLinkDelayByID - Unimplemented
  dPrint(ODTEST,"UNIMPLEMENTED!");
  return -1;
}
//*****************************************************************************

uint32_t connectionListGetLocalHubID(const TConnectionList *cl)
{
  //  dPrint(ODTEST,"Returning localHubID %08x", cl->localHubID);
  return cl->localHubID;
}
//*****************************************************************************

int connectionListReceiveData( TConnectionList *cl,
                               TConnectionListEntry *e)
{
  int result = CONNECTIONLIST_ERROR;
  int bytes;
  
  assert(cl != NULL);
  assert(e != NULL);
  
  if (!e->initialized) return CONNECTIONLIST_ERROR;
  
  // Lock the List
  pthread_mutex_lock(&cl->mutex);

  // Read the raw data (at maximum 'CONNECTIONLIST_TMP_BUFFERSIZE' at a time)
  // Must read like this, because the stringBuffer may already contain extra data.
  bytes = connectionReceiveData(e->connection, 
                                cl->tmpBuffer, 
                                CONNECTIONLIST_TMP_BUFFERSIZE);
  if ( bytes <= 0) {
    // Error check failed
    // Connection has failed, so close it! ( Return -1, caller destroys
    // the connection with Error handler )
    dPrint(4, "Reading stream failed! Returning error (Entry = %p)", e);
    result = CONNECTIONLIST_ERROR;
  
  } else {
    // Append data to connection buffer
    bufferAppend(&e->inputBuffer, cl->tmpBuffer, bytes);
    
    // Update byte counter
    //#warning Instrumentation missing.
    //e->receivedBytes += bytes;

    // Set result to be 0 (OK)
    result = CONNECTIONLIST_OK;
  }

  // Release the lock, we got data!
  pthread_mutex_unlock(&cl->mutex);
  
  return result;

}
//*****************************************************************************

int connectionListSendRawDataByEntry( TConnectionList *cl,
                                      const char *input, int count,
                                      TConnectionListEntry *target )
{
  int r;
  int result = CONNECTIONLIST_ERROR;
  
  assert(cl != NULL);
  assert(input != NULL);
  assert(count > 0);

  if (!target->initialized) return CONNECTIONLIST_ERROR;

  // For this operation, we must use mutex.
  pthread_mutex_lock(&cl->mutex);
  const unsigned int flags = target->flags;

  if (flags & connectionEntryFlag_DataInOutputBuffer) {
    //dPrint(ODTEST,"Entry already has data in output buffer. Pushing %u more bytes in outputBuffer",
    //       count);
    // The whole buffer was not sent! (The connection has blocked!)
    // So, If I can't flush the already cumulated buffer, I won't
    // even try to send the new buffer, but instead stuff it right away
    // to outputBuffer
    bufferAppend(&target->outputBuffer, (const unsigned char *)input, count);

    /////////////////////////////////////////////////
    // Check whether the target entry is DelayedLink.
    if (target->flags & connectionEntryFlag_DelayedLink) {
      
      // Push message frame in buffer.
      delayedLinkPushFrame(target->delayedLinkCtrl->delayedLinkInfo, count);
      
      // Result
      result = CONNECTIONLIST_OK; // Faking the result - no congestion here :)
      
    } else {
      // Result
      result = CONNECTIONLIST_SEND_DELAYED;
      
    }
    
    // Update the entry flag (This will allow the descriptor to be
    // handled for WRITE status readiness) Check only, as the flag
    // should stay on, until buffer is empty.
    assert(flags & connectionEntryFlag_SelectSetWrite); 
    
  } else {
    // Nothing in output buffer, so attempting to send.
    
    dPrint(ODDEBUG(1),"Sending data. (%d bytes, targetID: %08x, entry %p)", 
           count, target->connectionID, target);
    // Now, attempt to Send the immediate Data 
    // (if execution got here, the outputbuf is empty)
    r = connectionSendData(target->connection, 
                           (const unsigned char *)input,
                           count);
    
    // The return value must be checked because the connection may block!
    if ( r < 0 ) {
      // Some serious error happened! (Target has failed)
      dPrint(ODVINFO, "ConnectionSendData() has failed! The Send operation will abort.");
      result = CONNECTIONLIST_LINKERROR;
      
    } else if (r < count) {
      // The whole buffer was not sent! (The connection has blocked!)
      // So, stuff the 'leftover' data in the OutPut buffer.
      
      // \todo Check for overloaded output buffer.
      bufferAppend(&target->outputBuffer, (const unsigned char *)input+r, count-r);
      /*dPrint(ODTEST,"Unable to send the whole buffer! Sent %d / %d bytes.", r, count);
      dPrint(ODTEST,"Bytes in outputbuffer now: %d bytes", 
      bufferGetLength(&target->outputBuffer));*/
      
      // Add proper flags for OutputBuffer processing.
      connectionListUpdateEntryFlags(cl, 
                                     target, 
                                     target->flags | 
                                     connectionEntryFlag_SelectSetWrite | 
                                     connectionEntryFlag_DataInOutputBuffer);     
      
      result = CONNECTIONLIST_SEND_DELAYED; // OK
      
    } else {
      // All data sent. No further actions needed
      //dPrint(ODTEST,"All data successfully sent to connectionID 0x%08x! (%d bytes)", 
      //       target->connectionID, r);
      
      // Update byte counter
      //#warning Instrumentation missing.
      //      target->transmittedBytes += count;
      
      result = CONNECTIONLIST_OK; // OK
      
    }
  }

  // And, finally; Unlock the mutex
  pthread_mutex_unlock(&cl->mutex);
    
  return result;
}
//*****************************************************************************

int connectionListSendDataByEntry( TConnectionList *cl,
                                   TBuffer *input, int from, int count,
                                   TConnectionListEntry *target )
{
  return connectionListSendRawDataByEntry(cl, 
                                          (const char *)bufferGetPtr(input)+from, 
                                          count, 
                                          target);
}
//*****************************************************************************

int connectionListSendRawDataByID( TConnectionList *cl,
                                   const char *input, int count,
                                   uint32_t target )
{
  // Lookup entry
  TConnectionListEntry *targetEntry = connectionListGetEntry(cl, target, 0);
  
  // Correct entry found?
  if (targetEntry) {
    return connectionListSendRawDataByEntry(cl, input, count, targetEntry);

  } else {
    dPrint(ODTEST, "Requested targetID not found! (0x%08x)",
           target);
    
    return CONNECTIONLIST_TARGETNOTFOUND;
  }
}
//*****************************************************************************

int connectionListSendDataByID( TConnectionList *cl,
                                TBuffer *input, int from, int count,
                                uint32_t target)
{
  // Lookup entry
  TConnectionListEntry *targetEntry = connectionListGetEntry(cl, target, 0);
  
  // Correct entry found?
  if (targetEntry) {
    return connectionListSendDataByEntry(cl, input, from, count, targetEntry);
  
  } else {
    //dPrint(ODTEST, "Requested targetID not found! (0x%08x)",
    //       target);

    return CONNECTIONLIST_TARGETNOTFOUND;
  }
}
//*****************************************************************************

void connectionListSetLocalHubID(TConnectionList *cl, const uint32_t hubID)
{
  // Verify that the lower 16 bits are zero
  assert( (hubID & 0xFFFF) == 0);

  // Assign.
  cl->localHubID = (hubID & 0xFFFF0000);
  return;
}
//*****************************************************************************

int connectionListGetGatewayIDbyTargetID(TConnectionList *cl, 
                                         const uint32_t target,
                                         uint32_t *gateway)
{ 
  int result = CONNECTIONLIST_ERROR;
  
  // Lookup address from HUB entry table and transmit to correct connector HUB
  const TConnectionListEntry *entry = connectionListGetEntry(cl, target, 0);
  
  if (!entry) {
    uint32_t gatewayID;
    //dPrint(ODTEST,"The HUB entry was not known, attempting to search Routing table");
    
    // Ask for gatewayID for this targetHUB
    gatewayID = routingGetRoute(ID2HUBID(target));
    //dPrint(ODTEST,"Routing module returned '%08x' for search to ID '%08x'",
    //       gatewayID<<16, target);
    
    // Now, ask for targetEntry for this gatewayID
    entry = connectionListGetEntry(cl, gatewayID, 1);
    //dPrint(ODTEST, "Got entry %p (ID: '%08x') for gatewayID by target entry '%08x'",
    //      entry, 
    //      entry ? entry->connectionID : 0,
    //      target);
  }

  // Check final result
  if (entry) {
    *gateway = entry->connectionID;
    result = CONNECTIONLIST_OK;
  }
  
  return result;
}
//*****************************************************************************

TConnectionListEntry *connectionListGetConnectionEntryByID(TConnectionList *cl, 
                                                           uint32_t ID)
{
  return connectionListGetEntry(cl, ID, 0);
}
//*****************************************************************************

int connectionListFlushOutputBuffer( TConnectionList *cl, 
                                     TConnectionListEntry *target )
{
  int result = CONNECTIONLIST_ERROR;
  int r;
  int bytes_in_outputbuffer = bufferGetLength(&target->outputBuffer);
  
  // Check whether there really is content.
  if (bytes_in_outputbuffer > 0 || target->flags & connectionEntryFlag_DelayedLink) {
    dPrint(6,"Attempting to flush output buffer, with %d bytes of data.",
           bytes_in_outputbuffer);
    
    // Check whether this Link is Delayed
    if (target->flags & connectionEntryFlag_DelayedLink) {
      // Link is delayed.
      // verify that Flag and Pointer are consistent
      assert(target->delayedLinkCtrl->delayedLinkInfo != NULL);
      bytes_in_outputbuffer = delayedLinkGetExpiredSize(target->delayedLinkCtrl->delayedLinkInfo);
      dPrint(6,"We have %d bytes Expired in the outputBuffer", bytes_in_outputbuffer);
      
    }
    
    
    // Output buffer has data! So attempting to send it first.
    r = connectionSendData(target->connection, 
                           bufferGetPtr(&target->outputBuffer),
                           bytes_in_outputbuffer);
    
    if ( r < 0 ) {
      // Some serious error happened! (Target has failed)
      dPrint(3,"ConnectionSendData returned %d while flushing Output buffer!",
             r);
      
    } else {
      
      // Remove the succesfully sent amount from the output buffer and return
      bufferShift(&target->outputBuffer, r);
      
      // Update byte counter
      //#warning Instrumentation missing.
      //      target->transmittedBytes += r;
     
      // Handle specially if DelayedLink flag is set
      if (target->flags & connectionEntryFlag_DelayedLink) {
        int popped =
          delayedLinkPopExpiredSize(target->delayedLinkCtrl->delayedLinkInfo, r);
        dPrint(6,"Popped %d bytes from DelayedLink buffer", popped);

        // Set always delayed to force all data to go through delay.
        result = CONNECTIONLIST_SEND_DELAYED;

      } else {
        // Result, handle normally; based on returned value.
        result = r<bytes_in_outputbuffer?CONNECTIONLIST_SEND_DELAYED:CONNECTIONLIST_SEND_COMPLETE;

      }
    }
    
  } else {
    // dPrint(10,"No data in OutputBuffer for connection 0x%08x", 
    // target->connectionID);
    result = CONNECTIONLIST_OK;

  }

  // Return OK
  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

static uint32_t getNewLocalClientID(TConnectionList *cl)
{

  ++cl->localClientSequence;
  
  // Check
  //dPrint(ODTEST,"LocalHubID = %08x", cl->localHubID);
  assert((cl->localHubID & 0x0000FFFF) == 0);
  
  // Return fully qualified local ID.
  return (cl->localHubID | cl->localClientSequence);
}
//*****************************************************************************

inline static int connectionListAddEntry(TConnectionList *cl,
                                         const uint32_t aID,
                                         TConnectionListEntry *cle)
{
#if defined(USE_SELECT_IO)
  // Mark highest fd zero to force regeneration.
  cl->highest_set_fd = 0;
  
#elif defined(USE_EPOLL_IO)
# warning No implementation.
#endif
  
  return idMapAddEntry(&cl->connectionMap, aID, cle);
}
//*****************************************************************************

inline static int connectionListDeleteEntry(TConnectionList *cl,
                                            const uint32_t aID)
{
#if defined(USE_SELECT_IO)
  // Mark highest fd zero to force regeneration.
  cl->highest_set_fd = 0;
  
#elif defined(USE_EPOLL_IO)
# warning No implementation.
#endif
  
  return idMapDeleteEntry(&cl->connectionMap, aID);
}
//*****************************************************************************

inline static TConnectionListEntry *connectionListGetEntry(TConnectionList *cl,
                                                           const uint32_t aID,
                                                           const int direct_only)
{
  TConnectionListEntry *result = NULL;

  // Get entry.
  const TIDMapEntry *ime = idMapGetEntry(&cl->connectionMap, aID);
  
  if (ime) {
    // Got direct entry.
    result = (TConnectionListEntry *)ime->entry;
    
  } else if (!direct_only) {
    // Extract the AP part from the address
    const unsigned int target_hubid = ID2HUBID(aID);
    
    if (target_hubid == cl->localHubID) {
      /*     dPrint(ODTEST,
             "The AP of address %08x points to local AP; "
             "no reason to look from routing table.", aID);*/
      
    } else {
      
      //dPrint(ODTEST,"Target AP of %08x is not local, running query on the Routing module", 
      //       aID);
      
      
      const uint32_t tid = routingGetRoute(aID & 0xFFFF0000);
      
      // FIXME: Should this allow routing of single entries too?
      // Request target from full subnet, by masking out the lower bits.
      
      if (tid) {
        //dPrint(ODTEST,"Got GatewayID %08x from Routing module, looking for ConnectionListEntry",
        //       tid);

        // Got target ID.
        ime = idMapGetEntry(&cl->connectionMap, tid);
        
        if (ime) {
          // Got direct entry.
          result = (TConnectionListEntry *)ime->entry;
          //dPrint(ODTEST,
          //      "Found an entry for TargetID %08x through entry %08x in routing table, gateway at %p", 
          //     aID, tid, result);
          
        } else {
          // No target ID :(
          //dPrint(ODTEST,"No local entry found matching the returned GatewayID %08x", tid);
          
        }
      }
    }
  }

  return result;
}
//*****************************************************************************

int connectionListRegisterHub(TConnectionList *cl,
                              TConnectionListEntry *cle)
{
  int result = -1;
  dPrint(ODTEST,"At this point, we should be sending bits to remote Hub!");
  /*  
      THubCtrlProtocolHeader register_query;
    
      // Construct appropriate command
      hubctrlProtocolConstructHeader(&register_query,
      HUBCTRL_REQUEST_REGISTERHUB,
      HUBCTRL_FLAG_HUBNEGOTIATION);
  
      // Construct SET packet.
      hubctrlProtocolConstructHeader(&hdr_set,
      HUBCTRL_REQUEST_SETOWNID,
      HUBCTRL_FLAG_HUBNEGOTIATION);
      hdr_set.data.own_id = connectionListGetLocalHubID(cl);

      // Send QUERY packet, reply handled in HubCtrl
      result = connectionListSendRawDataByEntry(cl,
      (const char *)&hdr_query, 
      sizeof(hdr_query),
      entry);

      // Send SET packet, reply handled in HubCtrl
      result += connectionListSendRawDataByEntry(cl,
      (const char *)&hdr_set, 
      sizeof(hdr_set),
      entry);
  */
  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
#ifdef INCLUDE_OLD_SHIT

static int startHubNegotiation(TConnectionList *cl,
                               TConnectionListEntry *entry)
{
  int result;
  THubCtrlProtocolHeader hdr_query;
  THubCtrlProtocolHeader hdr_set;
    
  // Construct appropriate command packets
  hubctrlProtocolConstructHeader(&hdr_query,
                                 HUBCTRL_REQUEST_QUERYOWNID,
                                 HUBCTRL_FLAG_HUBNEGOTIATION);
  
  // Construct SET packet.
  hubctrlProtocolConstructHeader(&hdr_set,
                                 HUBCTRL_REQUEST_SETOWNID,
                                 HUBCTRL_FLAG_HUBNEGOTIATION);
  hdr_set.data.own_id = connectionListGetLocalHubID(cl);

  // Send QUERY packet, reply handled in HubCtrl
  result = connectionListSendRawDataByEntry(cl,
                                            (const char *)&hdr_query, 
                                            sizeof(hdr_query),
                                            entry);

  // Send SET packet, reply handled in HubCtrl
  result += connectionListSendRawDataByEntry(cl,
                                             (const char *)&hdr_set, 
                                             sizeof(hdr_set),
                                             entry);

  return result;
}
//*****************************************************************************

int connectionListSetLinkDelayByEntry(TConnectionList *cl, TConnectionListEntry *entry,
                                      const int delay_ms)
{
  int r,result = CONNECTIONLIST_ERROR;

  // Check whether we found an entry with this id.
  if (delay_ms > 0 && entry && 
      !(entry->flags & connectionEntryFlag_DelayedLink)) {

    ///////////////////////////////////////////////////////////////////////////
    // Inform the other end to carry out the same operation
    // (Send HubCtrl packet to initiate 'SET DELAY')
    THubCtrlProtocolHeader delay_set;
  
    // Construct appropriate command packet header, also set the 'delay'
    hubctrlProtocolConstructHeader(&delay_set,
                                   HUBCTRL_REQUEST_SETOWNLINKDELAY,
                                   0);
    delay_set.data.delay_ms = delay_ms;
    
    
    // Send SET packet, reply handled in HubCtrl
    r = connectionListSendRawDataByEntry(cl,
                                         (const char *)&delay_set, 
                                         sizeof(delay_set),
                                         entry);



    ///////////////////////////////////////////////////////////////////////////
    // Mark it as 'DelayedLink', and create a new LinkInfo entry.
    entry->flags |= connectionEntryFlag_DelayedLink;
    
    // Create new ctrl information entry
    entry->delayedLinkCtrl = (TDelayedLinkCtrl *)malloc(sizeof(TDelayedLinkCtrl));
    assert(entry->delayedLinkCtrl != NULL);
    memset(entry->delayedLinkCtrl, 0x00, sizeof(TDelayedLinkCtrl));
    
    // Create new LinkInfoEntry
    entry->delayedLinkCtrl->delayedLinkInfo = delayedLinkCreateLinkInfoEntry(delay_ms);
    assert(entry->delayedLinkCtrl->delayedLinkInfo != NULL);
    
    // Set the connectionList pointer (for thread usage)
    entry->delayedLinkCtrl->connectionList = cl;
    
    // Start handler thread for this entry
    entry->delayedLinkCtrl->linkMonitorThreadRunning = 1;
    r = pthread_create(&entry->delayedLinkCtrl->linkMonitorThread,
                       NULL, (void*)delayedLinkMonitorThread, entry);
    assert(r == 0);



    dPrint(2,"Succesfully set DELAY for H2H link %p to %dms",
           entry, delay_ms);
    
    result = CONNECTIONLIST_OK;
  }
  
  return result;
}
//*****************************************************************************

int connectionListSetLinkDelay(TConnectionList *cl, const int link_id, 
                               const int delay_ms)
{
  int result = CONNECTIONLIST_ERROR;

  // Find an entry from the table corresponding to 'link_id'
  TConnectionListEntry *entry = connectionListGetClientEntryByID(cl, link_id);
 
  // Do it
  result = connectionListSetLinkDelayByEntry(cl, entry, delay_ms);
 
  // Return the result
  return result;
}
//*****************************************************************************

int connectionListGetOutputBufferSizeByID(TConnectionList *cl, uint32_t target)
{
  int result = CONNECTIONLIST_TARGETNOTFOUND;

  // Lookup entry
  TConnectionListEntry *targetEntry = NULL;
  EConnectionListLookupStatus stat = lookupEntryByID(cl, target, &targetEntry);
 
  // The correct entry is now selected, so proceed with traditional Send!
  if (targetEntry != NULL && stat == connectionListLookup_Singletarget) {
    result = bufferGetLength(&targetEntry->outputBuffer);
  
  } else {
    dPrint(10, "Requested targetID not found! (%08x)",
           target);

  }

  return result;
}
//*****************************************************************************

int connectionListGetGatewayIDbyTargetID(TConnectionList *cl, 
                                         const uint32_t target,
                                         uint32_t *gateway)
     
{ 
  int result = CONNECTIONLIST_TARGETNOTFOUND;
  EConnectionListLookupStatus status;
  TConnectionListEntry *entry;

  // Trivial asserts
  assert(cl != NULL);
  assert(gateway != NULL);

  // Do a lookup
  status = lookupEntryByID(cl, target, &entry);
  if (status == connectionListLookup_Singletarget) { // FIXME: Only single handled!
    *gateway = entry->connectionID;
    result = CONNECTIONLIST_OK;
    dPrint(8,"Gateway resolved to be '%08x' for target '%08x'",
           *gateway, target);

  } else {
    dPrint(6,"Unable to resolve gateway for target '%08x'",
           target);
	
  }

  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//********** PRIVATE FUNCTION INTERFACE ***************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

static int delayedLinkMonitorThread(TConnectionListEntry *cle)
{
  
  // Init easier-to-reference pointers and other local variables
  const int min_delay = 10;
  const int max_delay = 2000;
  int r,sleep_until_recheck = 100;
  
  TDelayedLinkCtrl *ctrl = cle->delayedLinkCtrl;
  assert(ctrl != NULL);
  
  TDelayedLinkInfo *link = ctrl->delayedLinkInfo;
  assert(link != NULL);
  
  TConnectionList *list = ctrl->connectionList;
  assert(list != NULL);
  
  
  // Run checker loop.
  while (ctrl->linkMonitorThreadRunning) {
    // Lock the List, preventing new data while processing
    pthread_mutex_lock(&list->mutex);
    
    // Get pointer to topmost frame in the delayedlink table
    sleep_until_recheck = delayedLinkGetTimeMsUntilNextExpiredFrame(link);
    dPrint(10,"Got sleeptime %dms", sleep_until_recheck);
    
    if (sleep_until_recheck == 0) {
      // Frame(s) already expired! So calling the flusher
      dPrint(10,"Got expired frames, so call flush()");
      r = flushOutputBuffer(list, cle);
      
    } else if (sleep_until_recheck < 0) {
      // No frames in queue, so we can sleep maximum time
      sleep_until_recheck = link->linkDelayMs + 1;
      dPrint(10,"No messages in Queue, sleeping for %d + 1ms",
             sleep_until_recheck-1);
      
    }
    
    // Unlock
    pthread_mutex_unlock(&list->mutex);
    
    // Limit the minimum and maximum sleep of the loop to min=10ms, max=2000ms
    if (sleep_until_recheck < min_delay) sleep_until_recheck = min_delay;
    else if (sleep_until_recheck > max_delay) sleep_until_recheck = max_delay;
    
    // Commence sleeping the thread
    dPrint(10,"Limit-Checked final 'sleep_until_recheck' = %d, Sleeping....", 
           sleep_until_recheck);
    ownSleep_ms(sleep_until_recheck);
  }
  
  return 0;
}
//*****************************************************************************

static int flushOutputBuffer( TConnectionList *cl, 
                              TConnectionListEntry *target )
{
  int result = CONNECTIONLIST_ERROR;
  int r;
  int bytes_in_outputbuffer;

  bytes_in_outputbuffer = bufferGetLength(&target->outputBuffer);
  
  if (bytes_in_outputbuffer > 0 || target->flags & connectionEntryFlag_DelayedLink) {
    dPrint(6,"Attempting to flush output buffer, with %d bytes of data.",
           bytes_in_outputbuffer);
    
    // Check whether this Link is Delayed
    if (target->flags & connectionEntryFlag_DelayedLink) {
      // Link is delayed.
      // verify that Flag and Pointer are consistent
      assert(target->delayedLinkCtrl->delayedLinkInfo != NULL);
      bytes_in_outputbuffer = delayedLinkGetExpiredSize(target->delayedLinkCtrl->delayedLinkInfo);
      dPrint(6,"We have %d bytes Expired in the outputBuffer", bytes_in_outputbuffer);
      
    }
    
    
    // Output buffer has data! So attempting to send it first.
    r = connectionSendData(target->connection, 
                           bufferGetPtr(&target->outputBuffer),
                           bytes_in_outputbuffer);
    
    if ( r < 0 ) {
      // Some serious error happened! (Target has failed)
      dPrint(3,"ConnectionSendData returned %d while flushing Output buffer!",
             r);
      
    } else {
      
      // Remove the succesfully sent amount from the output buffer and return
      bufferShift(&target->outputBuffer, r);
      
      // Increment total byte counter
      target->transmittedBytes += r;
     
      // Handle specially if DelayedLink flag is set
      if (target->flags & connectionEntryFlag_DelayedLink) {
        int popped =
          delayedLinkPopExpiredSize(target->delayedLinkCtrl->delayedLinkInfo, r);
        dPrint(6,"Popped %d bytes from DelayedLink buffer", popped);

        // Set always delayed to force all data to go through delay.
        result = CONNECTIONLIST_SEND_DELAYED;

      } else {
        // Result, handle normally; based on returned value.
        result = r<bytes_in_outputbuffer?CONNECTIONLIST_SEND_DELAYED:CONNECTIONLIST_SEND_COMPLETE;

      }
    }
    
  } else {
    // dPrint(10,"No data in OutputBuffer for connection 0x%08x", 
    // target->connectionID);
    result = CONNECTIONLIST_OK;

  }

  // Return OK
  return result;
}
//*****************************************************************************

static EConnectionListLookupStatus lookupEntryByID(TConnectionList *cl, 
                                                   uint32_t target,
                                                   TConnectionListEntry **entry)
{
  EConnectionListLookupStatus result = connectionListLookup_NoTarget;
  TConnectionListEntry *targetEntry = NULL;
  unsigned short int targetHUB = ID2HUBID(target);
  unsigned short int targetClient = ID2CLIENTID(target);
  
  if (targetHUB == cl->hubID) {
    // This is a local transmission, so just get the entry from local table:

    if (targetClient == CONNECTIONLIST_CLIENTID_BROADCAST) {
      result = connectionListLookup_BroadcastClients;
      
    } else {
      targetEntry = connectionListGetClientEntryByID(cl, targetClient);
      if (targetEntry) 
        result = connectionListLookup_Singletarget;
    
    }

  } else if (targetHUB == CONNECTIONLIST_HUBID_BROADCAST) {
    // Broadcast to ALL HUBS
    result = connectionListLookup_BroadcastHubs;

  } else {
    // Lookup address from HUB entry table and transmit to correct connector HUB
    targetEntry = connectionListGetHubEntryByID(cl, targetHUB);
  
    if (targetEntry == NULL) {
      int gatewayID;
      dPrint(4,"The HUB entry was not known, attempting to search Routing table");
      
      // Ask for gatewayID for this targetHUB
      gatewayID = routingGetRoute(targetHUB);
      dPrint(4,"Routing module returned '%04x' for search to '%04x'",
             gatewayID, targetHUB);

      // Now, ask for targetEntry for this gatewayID
      targetEntry = connectionListGetHubEntryByID(cl, gatewayID);
      dPrint(4, "GetHubEntryByID returned %p (ID: '%08x', key: '%04x') for gatewayID.",
             targetEntry, 
             targetEntry ? targetEntry->connectionID : 0,
             targetEntry ? targetEntry->key : 0);

      
      // Did we finally get the entry? If so, update result.
      if (targetEntry) 
        result = connectionListLookup_Singletarget;
    
    } else {
      result = connectionListLookup_Singletarget;   
    
    }
  }

  // Store the found entry (even if it's NULL)
  *entry = targetEntry;

  // Return the result
  return result;
}
//*****************************************************************************
#endif
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
